/*
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "OfmsMath.h"
#include <cassert>
//using namespace OpenFms;

static const float half_rho = .6f; /**< Half the denity of air in kg/m^3 */

/* 
 * Convert [-1,1] -> [0, 0, ... 1, 1 ]
 *
 * @param value Value to convert
 * @return Returns the converted value
 */
float toThrustScale( float value )
{
	value *= .625f;
	value += .5f;
	if( value >= 1.f ) return 1.f;
	if( value <= 0.f ) return 0.f;
	return value;
}

/**
 * Sets all forces and torques in a in a ModelSimulator_forces_t structure
 *
 * @note The size field of the structure should be set.
 * @param[in,out] forces Forces structure set to zero
 */
void clearForces( ModelSimulator_forces_t &forces )
{
	assert( forces.size == 28 );

	uint32_t size = forces.size;
	memset(&forces,0,size);
	forces.size=size;
}

/**
 * Adds a force to a forces structure
 *
 * @param[in,out] target Forces structure to add force to
 * @param[in] force Force to add
 * @param[in] position Coordinate where force is applied
 */
void addForce( ModelSimulator_forces_t &target, const btVector3 &force, const btVector3 &position )
{
	target.force_x += force.getX();
	target.force_y += force.getY();
	target.force_z += force.getZ();

	btVector3 torque = position.cross(force);
	target.torque_x += torque.getX();
	target.torque_y += torque.getY();
	target.torque_z += torque.getZ();
}

/** 
 * Caclulates the force wind acts on a surface 
 *
 * @param normal Vector perpendicular to the surface, does not need to be unit length.
 * @param wind Vector pointing in the directio of the wind, lengs of vector is wind speed [m/s]
 * @param area Effective area of the surface
 * @return Retuns the force acten upon the surface [N]
 */
btVector3 forceOnSurface( const btVector3 &normal, const btVector3 &wind, float area )
{
	// Part of wind perpendicular into the surface.
	btVector3 perpWind = -normal * ( normal.dot(wind)/normal.length2() );

	return (area*half_rho)*(perpWind*perpWind);
}

